はじめに

ROS（Robot Operating System）のプログラミングは、とても簡単です。その一番大きな理由は、ROSはOSと銘打っているけど実はただの通信ライブラリで、規模が小さいので習得が容易なこと。次に挙げられるのは、ツールやパッケージが揃っているので、我々がプログラミングしなければならない部分が小さいこと。あと、ROSの主たる開発言語のC++が順調にバージョン・アップして、とても使いやすくなったこと。

公平を期すために、ROSプログラミングの問題点についても挙げてみましょう。ドキュメントに載っているサンプルがC++の古い規格（C++03）で書かれているので、プログラミングが面倒くさそうに見えてしまうこと。最新であるC++14を使って簡潔に簡単にプログラミングする場合向けのサンプルは、見つからないんですよね。他は、文書、特に日本語の文書が少ないこと1。ただでさえ少ない文書が要素技術単位で細切れになっていて、開発の全体像を掴みづらいとも感じます。

簡単に使えてとても便利なROSなのに、もしこの程度の問題で嫌われてしまったらもったいない。だから、C++14を使用したサンプルを作成してきました。そして本稿で、ROSプログラミングについて、実際の開発に則した形で解説します2。

ロボットを動かしてみる

まずは肩慣らし。ロボットを動かしてみましょう。

TurtleBot

本稿は、ロボットとしてTurtleBotを使用します。TurtleBotはROSアプリケーションの開発プラットフォームで、iRobot社のCreateやYujin Robot社のKobuki、Microsoft社のKinect、Asux社のXtion PRO LIVEといった市販のハードウェアで構成される、オープン・ソース・ハードウェアのロボットです。

なぜ、TurtleBot？

本稿がTurtleBotを使用する理由は、安くて簡単で情報が豊富なためです。

安い
20万円程度で購入できます。もしiRobot社のルンバの中古品とXbox360から取り外したKinect、ベニヤ板があるなら、公開された設計書に従って自作することもできます。
簡単
TurtleBotに対応した多数のROSパッケージがあるので、開発範囲を小さくできます。たとえば、ROS WikiのTurtleBotのページの記述通りにコマンド入力すれば、プログラミングせずとも地図作成や自律移動ができます。
情報が豊富
多数の開発者が、安くて簡単なTurtleBotでROSアプリケーションを開発しています。だから、情報も豊富です。たとえばLearn Turtlebot and ROSでは、セットアップからスマートフォンで呼び出し可能なTurtleBotによるコーヒー配膳システムの完成まで、丁寧に解説してくれています。トラブルが発生しても、インターネットを検索すれば解決方法が見つかりますし。

セットアップ

TurtleBot Installationに従い、TurtleBotに必要なソフトウェアをセットアップしてください。

実機がない場合は、シミュレーターで

TurtleBotを用意できなくても、ご安心ください。ROSに対応した、Gazeboというシミュレーターがあります。Gazebo Bringup Guideに従って、TurtleBotのシミュレーターを起動してみてください。内蔵された物理エンジンが、いい感じに物理演算してくれます。

一点だけ、気をつけてください。Gezeboは初回起動時にオブジェクトのデータをダウンロードしますから、画面が表示されるまでかなり時間がかかります。気長に待っていただければ以下の画面が表示され、TurtleBotの実機がなくてもROSを試せるようになります。

Gazebo

ROSの仕組み

プログラミングに入る前に、簡単にROSについて説明させてください。ROSはただの通信ライブラリなのですけれど、どんな通信をしてどのようにロボットを動かすのか、そもそもどうして通信ライブvラリがロボットのソフトウェア開発に必要になるのかが分からないと、モヤモヤしますもんね。

ロボットのソフトウェア開発と通信

少し遠回りになりますけど、電子工作相当の低水準の話から。ロボットはセンサーやモーターで構成されているわけですけど、これらはアナログな電気で制御されます。センサーはその測定結果を電圧として渡してきますから、専用の回路でデジタルに変換しなければなりません。DCモーター（普通のモーター）の力の制御も電圧で、でも、電子回路で電圧そのものを制御するのは難しい。だから、PWM（Pulse Width Modulation）制御と呼ばれるスイッチを高速にオン/オフする方式で、DCモーターの力を調節しなければなりません。

で、これがやたらと面倒くさい。だって、高速でスイッチをオン/オフするには、CPUのタイマー割り込みを使った面倒なコードが必要なのですから。なので、例えば電子工作で人気のArduinoでは、analogWrite()というPWM制御用のライブラリを用意し、タイマー割り込みを隠蔽しています。サーボ・モーター（ロボット・アームの関節等で使われる、角度を指定できるモーター）のようにスイッチをオン/オフする周波数が定められている場合向けに、一つのタイマーで複数のサーボ・モーターを制御するServoクラスも用意されています。

でも、ライブラリが揃っても、まだ他に問題があるんです。ロボット・プログラムの動作は「センサーの値を読む→読み取った値に合わせてモーター等を制御する」の繰り返しで、それをコードで表現すると以下になります。

Rate loop_rate(30)  // 30Hz（秒に30回）でループできるように、ループ・レートを設定。
while (1) {  // 無限ループ。
  auto accelerator_opening = analogRead(ACCELERATOR_PIN);  // アクセル開度のセンサーから情報を取得。
  analogWrite(MOTOR_PIN, accelerator_opening);  // モーターの速度を設定。
  
  loop_rate.sleep();  // ループ・レートに合わせてスリープする。
}

上のコードは、一定の周期でループが回るようになっています。たとえば秒30コマ撮るビデオ・カメラのデータを使うプログラムなら秒30回処理が動けば十分なので、ループを30Hzに制限します。でも、この周期って、デバイスによって違うんですよ。たとえば、細かい制御が可能なブラシレス・モーター3制御の最小タイミングはモーターが少し回転するたびで、だから、はるかに大きな周波数を設定しなければなりません。でも、センサーやモーターが増えると複数の周波数に対応しなければならなくなって、これを一つのループで表現するとコードが複雑になるのでやりたくない。PWM制御ライブラリのようなタイマー割り込みでの実現も、作成コストが高いから避けたい。マルチ・スレッドで複数のループを作る方式は、制御が不正確になってしまうのでダメ。

だから一般に、この複数の周波数問題は、CPUを分けることで解決します。CPUと言ってもPCに入ってるIntel Core i7とかの高級品ではなくて、16MHz動作の8bitでメモリ2KB（単位に注意してください）とかの、マイクロ・コントローラーと呼ばれる安価なやつです。これを、センサーやモーター毎につけて、それぞれのデバイスに適した周波数でループを回して制御すればよい。ロボットを分散型で制御するわけですね。

でも、センサーとマイクロ・コントローラー、モーターとマイクロ・コントローラーのような小さな単位を組み合わせて大きなロボットを作るのは、組み合わせが多すぎるので大変。だから、センサーやモーターのマイクロ・コントローラーを、デジタルなので管理が容易なシリアル通信でつなげて、移動ユニットのような形でコンポーネント化します。このコンポーネントの組み合わなら、ロボットの作成は楽になりそう……なのですけれど、残念ですけどまだ課題があります。

というのも、シリアル通信ではビット列を送受信しますが、そのビット列にどのようなデータを使うのかは利用者の自由なんです。だから、A社の移動ユニットとB社の移動ユニットではシリアル通信する内容が異なっていて、コンポーネントを組み合わせたり交換したりはできません。あと、人工知能が行き先を示すようなハードウェア無関係のソフトウェア・コンポーネントでも、問題が発生します。自由度が高いソフトウェア・コンポーネント同士を自由度が低いシリアルで通信させるのは無意味ですから。というわけで、通信はもっと抽象度が高いネットワークで実現すべきでしょう。

まぁ、ネットワーク通信でも、HTTPでどんな内容のホームページでも送信できるように、自由なデータを送れちゃう。でも、ソフトウェアで階層的に実現されている層が多いですから、送信内容の規格化は容易です。各種業界団体が専用のXML規格を定めるように、移動を表現するデータ構造（X軸とY軸とZ軸の移動量と、X軸とY軸とZ軸の回転量）を定めてあげれば、A社の移動ユニットとB社の移動ユニットが交換可能になります。

でも、先ほど例に出てきたHTTPそのものだとセッション単位で1対1の接続なので、センサーから取得した同じデータを複数回送信することになって効率が悪そう。あと、ブラウザからリクエストが来たらデータを送る方式だと、センサーというデータを流し続ける仕組みと合致しない。メッセージがXMLだと、プログラムを書くのが大変そうで嫌。構造体のような、もっとプログラミング言語寄りの楽なやり方が欲しい。うん、やっぱり、既成品じゃあダメですね。

というわけで、ロボット専用の通信ライブラリが必要という結論になりました。ROSは、このロボット専用の通信ライブラリなのです。

Publish/Subscribeの、ロボットに適した通信を実現する

さて、そのロボット専門の通信ライブラリであるROSですが、実はその内部では先ほど文句をつけたHTTPも使用しています。でも、外から見えるROSの動作はHTTPとは大きく異なっていて、さすがはロボット専用な感じ。HTTPはブラウザからGETやPOSTというリクエストを受け取ったらサーバーがコンテンツを送る方式ですけれど、ROSでのサーバーはメッセージ流しっぱ、クライアントが必要に応じて読み取るという方式です。いわゆるPublish/Subscribe。センサーに適した、同じデータが複数回流れない効率のよい動作です。

で、このメッセージを流したり受け取ったりするプログラムの単位をROSではノードと呼び、通常はプロセスとして実現します。あと、種々雑多なメッセージが流れると取捨選択が必要になってしまいますから、同じメッセージをトピックとしてまとめます。このノードやトピックは、管理しやすいように階層で管理します。Microsoft社のKinectを管理する/kinectというノードが、取得した3D点群のメッセージを/3dsensor/depth/pointsというトピックに流す感じ。で、3D点群から障害物を検知してハンドル操作する/steeringノードや、3D点群を後で分析できるようにディスクに保存する/pointcloud\_to\_pcdノードが、先ほどの/3dsensor/depth/pointsというトピックからメッセージを読み込んでそれぞれの処理を実施します。

この、メッセージをトピックに送ることをpublish、トピックからメッセージを受け取ることをsubscribeと呼びます。出版と購読ですね。先ほどは書きそびれましたけど、3D点群から障害物を検知する/steeringノードは、多分、右へ行けとか左に行けというメッセージをpublishします。で、このメッセージを/move_unitノードがsubscribeする。さらに、/steeringノードは/move_unitノードがpublishする機体の情報をsubscribeして、移動が可能かどうかの判断に使っているでしょう。ROSのノードは対等で、HTTPのようなサーバーとかクライアントという区分けは存在しないんです。

サービスで、1対1の同期通信にも対応する

Publish/Subscribeでのメッセージの流れはパブリッシャーからサブスクライバーへの1方向のみで、非同期です。センサーが情報を垂れ流すロボット制御におけるほとんどの処理はPublish/Subscribeが適しているのですけれど、もしメッセージの内容が変わらないならpublishし続けるのは無駄が多いですし、メッセージを受け取ったかどうかを確認したい場合は非同期だと困ります。

たとえば、ロボットの名前を渡したいとします。名前はいつまでも変わらないですから、名前を何度もpublishするのは無駄です。「あなたの名前は？」「ロボット三等兵です」のようなやりとりの方が、「私の名前はロボット三等兵です」という同じ内容を繰り返し叫び続けるより適切でしょう。

カメラの動作モード設定のような場合も、Publish/Subscribeは使えません。動作モードを切り替えるメッセージをpublishしたけど、/cameraノードはsubscribeしそこねた（RDBMSでトランザクションのようなプログラムを書いている人には信じられないかもしれませんけど、ロボットでは十分にありえる話）。だから、/cameraノードは指定した動作モードとは異なるモードの画像をpublishする。でも、動作モード切り替えを指示したノードは、それを知るすべがない。だから、画像の解析に失敗しちゃう。

というわけで、ROSは、Publish/Subscribe方式に加えてサービスと呼ぶ1対1の同期通信も提供します。残念なことにROSの用語はテキトーで、Publish/Subscribeの場合はトピックとメッセージという用語で、サービスの場合はサービスとサービスという混乱する用語になっています（正しくは、Publish/Subscribeの場合はtopicとmsgで、サービスの場合はserviceとsvcなんですけれど）。だから、わけのわからない説明で申し訳ありませんが、ノードは、サービスからサービス・リクエストを受け取ってサービス・レスポンスを返すことで1対1の同期通信をするという説明になります。

メッセージやサービスを、簡単に定義/使用可能にする

メッセージは*.msg、サービスは*.svcというファイルで定義します。文法は、struct宣言みたいな感じ（実際にプログラミングするところで詳細を述べます）。そして、*.msgや*.svcはビルド時にC++の構造体のコードに変換されます。普通のコードでメッセージやサービスを作れるわけで、専用のAPIを使わなければならないXML作成より簡単です。トピックにpublishされたメッセージを表示するツールもあって、XMLと同様に人間が目で内容を確認することもできます。

ここまでをまとましょう。ROSプログラムは、ノードと呼ばれるプログラムの集合になります。ノードは、トピックにメッセージをpublishしたりトピックからメッセージをsubscribeしたりして、非同期通信で処理を進めます。あまり使いませんけど、サービスという1対1の同期通信もあります。メッセージやサービスは、struct宣言みたいな感じで定義して、我々が書くC++のコードからはstructに見えるようになります。ほら、「ロボットのソフトウェア開発と通信」で挙げたHTTPとXMLでロボットする場合の問題が、すべて解決されたでしょ？

マルチスレッドとスレッド間通信で、高パフォーマンスも実現する

でも、どれだけ良いものであっても、遅すぎるなら使えません。ROSではノードからノードへメッセージがネットワークで送られていくわけで、しかもそのメッセージには画像とか3D点群とかの大きなデータが含まれるみたい。だとしたら、パフォーマンス面がちょっと不安かも……。

ご安心ください。ROSはパフォーマンスも考慮しています。ROSは、複数のノードをマルチ・スレッドで動作させて、ネットワークではなくメモリでスレッド間通信するNodeletという仕組みも提供しているんです。しかも、コードを修正しなくても、通信相手のノードが別のプロセスや別のコンピューターにあるなら自動でネットワーク通信に切り替えてくれます。これなら、パフォーマンスも安心ですよね。

あと、マルチ・スレッドにまつわる面倒事はNodeletが解消してくれますから、マルチ・スレッドであってもプログラミングは難しくありません。本稿では、この速くて簡単なNodeletを使用してプログラミングしていきます。

ついでに、ディストリビューションとバージョンの話

ROSとROSに対応したパッケージ群を指して、ROSディストリビューションと呼びます。UbuntuやDebianのような、Linuxのディストリビューションと同じ感じ。ディストリビューションの目的は、組み合わせが自由な整合性のあるパッケージのセットを用意することです。

ROSディストリビューションは、毎年新バージョンがリリースされます。本稿を執筆している2016年2月の最新ディストリビューションの名前は「ROS Jade Turtle」で、2016年5月に出る予定の次のディストリビューションは「ROS Kinetic Kame」です。JからKへと、名前の最初のアルファベットがABC順で進んでいきます。なお、ROSディストリビューションのバージョンは、「ROS Jade」や「ROS Kinetic」のように省略して書くことが多いようです。あと、偶数年にリリースされるROSディストリビューションはLTS（Long Term Support）版で、サポート期間が5年間になります（奇数年リリースのサポート期間は2年）。2014年リリースの「ROS Indigo Igloo」と2016年リリース予定の「ROS Kinetic Kame」が、LTSになります。

本稿は、ROS Indigoを使用します。最新のROSディストリビューションへの対応が追いついていないパッケージが多いことと、やはりサポートが長いLTSを使いたいですためです。なお、ROS Wikiの日本語のページでは、ROS Indigo Iglooのさらに前のROS Hydro Medusa向けの記事が多く存在します。ROS Wikiの通りにやってもうまく行かないという場合は、その文書がどのROSディストリビューションを対象にしているのかを確認してみてください。

roslaunch

さて、いよいよプログラミング……の前に、既存のROSプログラムを動かしてみましょう。プログラムは完成したけど動かし方が分からないなんて、笑い話にもならないですもんね。ROSのノードは、直接起動、rosrunで起動、roslaunchで起動の3つの方法で起動できます。本稿のおすすめはroslaunchなのですけれど、その理由を理解していただくためにも、順を追って説明していきます。

直接起動する

通常のROSのノードは、実行可能なプログラムです。だから、直接起動できます。ターミナルを3つ開いて、それぞれで以下のコマンドを実行してください。

$ roscore
$ /opt/ros/indigo/lib/turtlesim/turtlesim_node
$ /opt/ros/indigo/lib/turtlesim/turtle_teleop_key

画面は以下のようになって、turtle_teleop_keyを実行したターミナル#3でカーソルキーを押すと、TurtleSimウィンドウの亀が動きます。

TurtleSim

少しだけ解説を。ノード同士が通信するには、相手の情報が必要です。そのために、ROSではMasterというプログラムが動いていて、ノードの情報を集中管理しています。インターネットにおけるDNS（動的な登録ができるので、ダイナミックDNSの方が適切かも）みたいなモノですね。他にも、柔軟なパラメーター管理のために、Parameter Serverというプログラムも動いています。そうそう、ログ管理も。これらのプログラムを1つずつ動かしていくのは大変なので、ターミナル#1で実行したroscoreというコマンドで一気に起動できるようになっています。あと、ターミナル#2と#3で/opt/ros...のように長々と入力しているのは、ROSはプログラムはディレクトリ分割で管理されていて、プログラムのあるディレクトリ群が多すぎてPATH環境変数に登録できないためです。

そうそう、ROSのプログラムはCtrl-c（キーボードのコントロール・キーを押しながらcを押す）で終了できます。起動したプログラムをすべて終了させて、次に進んでください。

rosrunで起動する

上の直接起動する方式だと、パスを入録するのが面倒です。この問題を解決するために、ROSはrosrunというコマンドを提供します。

$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun turtlesim turtle_teleop_key

これで、少し楽になりました。

roslaunchで起動する

でも、rosrunでもまだまだ面倒くさい。roscoreせずにrosrunするとエラーが表示されますから、roscoreが動いているかどうかは検出できるはず。だったら、何もしなくても必要に応じてroscoreを自動で実行させて欲しい。ターミナルを複数開くのも、コマンドを複数回も入力するのも、終了させるためにCtrl-Cを何度も叩くのも面倒です。

この問題はシェル・スクリプトを書けば解消できそうなのですけれど、できれば、汎用言語であるシェル・スクリプトではなく、ROSに特化した言語で楽にやりたい。Javaプログラムのビルドを、シェル・スクリプトではなくてAntやMaven（.NET FrameworkならMSBuild、RubyならRake、私が大好きなClojureならLeiningen）でやる方が楽なのと同じ。というわけで、roslaunchを使いましょう。roslaunchは、ROSプログラム起動用のコマンドと、そのコマンドで処理可能な言語（フォーマットはXML）の文法の両方を指します。

ただ、これまでで例として使用してきたturtlesimは、roslaunchのlaunchファイルを提供してくれていないんですよ（roslaunchの文法で書かれたファイルをlaunchファイルと呼びます）。なので、本当はセットアップされたパッケージ対してやっていはならないことなのですけれど、launchファイルを追加してみます。

ターミナルで以下のコマンドを実行して、

$ sudo mkdir /opt/ros/indigo/share/turtlesim/launch
$ sudo gedit /opt/ros/indigo/share/turtlesim/launch/teleop.launch

以下の内容のファイルを作成してください4。

<launch>
  <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_1"/>
  <node pkg="turtlesim" type="turtle_teleop_key" name="turtlesim_1_teleop_key"/>
</launch>

roslaunchの場合の起動の仕方は、以下の通り。必要なターミナルは一つだけです。

$ roslaunch turtlesim teleop.launch

TurtleSim

うん、これなら楽ちんですね5。Ctrl-c一回ですべて終了できましたし。

catkin

ついにプログラミング……の前に、パッケージの作成です。ROSアプリケーションは、パッケージと呼ばれる単位で管理されます。我々がこれから作成するROSアプリケーションでも、パッケージのお約束を守らねばなりません。このお約束をうまいこと守りながらビルドしてくれるツールとして、catkinというビルド・ツールがあります。

ワーク・スペースの作成

前にも述べましたが、ROSアプリケーションは、複数のノードがメッセージを送り合うことで動作します。で、これから我々が開発するノードが使用するメッセージは、他のパッケージで定義されたものかもしれません。この「他のパッケージ」をうまいこと利用できるようにする手段が「ワーク・スペース」なのですけど、ROSのプログラミングをまだ始めていない状態ではナンノコッチャって感じ。なので、他のケースで思考実験してみましょう。

題材は、C++のヘッダー・ファイルのインクルードにします。あなたのソース・コードは、もちろん#include <stdio>できます。標準ライブラリのヘッダーですもんね。あと、あなたが今日作成したfor_this_project.hも#include "for_this_project.h"できるでしょう。ファイルをインクルード可能なディレクトリに置くでしょうからね。

でも、昔やったプロジェクトのgood_old_days.hだったら？　この場合は、コンパイラにインクルード・ディレクトリを指定するオプション（-I）を付けなければならないでしょう。でもね、追加したいディレクトリがいっぱいあったら？　オプションを何度も書くのは面倒くさいので、どーにかしたい。幸いなことにROSをインストールしたUbuntuはLinuxなのでコマンドがいっぱいあって大抵のことはできますから、たとえば、

$ g++ `find ~/my_projects \( -name include -a -type d \) -print | sed "s/^/-I/g" | paste -s` x.cpp

と実行すれば、a_long_time_ago.hがmy_projectsの下のプロジェクト用ディレクトリの下のincludeディレクトリにあるなら#include "a_long_time_ago.h"できます。でもまぁ、include以外のフォルダに置かれていると駄目なんですけど……。

はい、思考実験終わり。この思考実験から得られる結論は、同じルールに従うプロジェクトのディレクトリを、あるディレクトリの下にまとめて格納すると便利ってことです。で、catkinのルールに従うパッケージ用のディレクトリを格納して便利にやってやろうぜというディレクトリが、catkinのワーク・スペースというわけ。

では、その便利なワーク・スペースを作りましょう。ターミナルで以下のコマンドを実行することで、ワーク・スペースを作成できます。

$ source /opt/ros/indigo/setup.bash
$ cd <ワーク・スペースの親ディレクトリ>
$ mkdir -p <ワーク・スペース名>/src
$ cd <ワーク・スペース名>/src
$ catkin_init_workspace
$ cd ..
$ catkin_make
$ echo 'source <ワーク・スペースの親ディレクトリ>/<ワーク・スペース名>/devel/setup.bash' >> ~/.bashrc

最後のechoしている1行では、ターミナルを開いたときにこのワーク・スペース向けの環境設定をするように指示しています（お好きなテキスト・エディタで~/.bashrcを編集してもOK）。これをしておかないとせっかく作成したワークスペースが使われませんので、注意してください。

上のコマンド中の、<ワーク・スペースの親ディレクトリ>は、どこでも構いません。私は~/Documents/Projectsの下に開発プロジェクトのファイルを置くと決めているので、ここにしました。<ワーク・スペース名>もなんでも構わないのですが、一般的にcatkin_wsとすることが多いようで、私もこの名前にしました。というわけで、私の環境では~/Documents/Projects/catkin_wsディレクトリがワーク・スペースになりました。以降の記述では、このワーク・スペースを<catkin_ws>と記述します。頭の中で皆様のワーク・スペースに置き換えてくださるよう、お願いいたします。

パッケージの作成

ワーク・スペースができたので、パッケージを作成しましょう。ターミナルから以下のコマンドを実行すれば、パッケージを作成できます。

$ cd <catkin_ws>/src
$ catkin_create_pkg <パッケージ名> [<依存するパッケージ名1> [<依存するパッケージ名2>]]

最初にcdする先は、srcディレクトリです（私の環境では~/Documents/Projects/catkin_ws/src）。catkin_create_pkgをワーク・スペース直下で実行しないように、注意してください（まぁ、間違えて実行しても、生成されたディレクトリを消せば大丈夫なのですけれど）。

2行目のcatkin_create_pkgが、パッケージの雛形生成です。オプションの二番目以降の<依存するパッケージ名>は、そんなの作る前に分かるはずが無いですし、入れると雛形に余計な記述が増えて混乱したりもするので、使うことが確実なC++開発用パッケージであるroscppだけでよいでしょう。

で、さて、必須オプションの<パッケージ名>は、何にしましょうか？　本稿で作成するプログラムは、ロボットの周囲360°の3D点群や対象物の周囲360°からの3D点群を生成するというものです。キーワードは360°ですね。ただ、プログラミングの世界では180°をπにしたラジアンを使うのが一般的なので、2π≒2×3.14＝6.28を名前にしましょう。というわけで、catkin_create_pkg six_point_two_eight roscppしてパッケージを作成してください。

package.xmlとCMakeLists.txtの編集

生成されたパッケージ・ディレクトリをlsしてみると、以下のようになっています。

$ ls -F six_point_two_eight
CMakeLists.txt  include/  package.xml  src/

includeは*.hファイルを格納するディレクトリ、srcは*.cppファイルを格納するディレクトリです。とりあえずこの2つは無視してください。パッケージの情報を表すpackage.xmlファイルと、ビルドを制御するCMakeLists.txtファイルは、今のうちに編集しておきましょう。

package.xml

コメントを削除し、埋めるべきところを埋めると、以下になります。

<?xml version="1.0"?>
<package>
  <name>six_point_two_eight</name>
  <version>0.1.0</version>
  <description>Getting 360 degree PointCloud.</description>
  <maintainer email="rojima1@gmail.com">OJIMA Ryoji</maintainer>
  <license>BSD</license>

  <buildtool_depend>catkin</buildtool_depend>
  
  <build_depend>roscpp</build_depend>
  
  <run_depend>roscpp</run_depend>

  <export>
  </export>
</package>

CMakeLists.txt

自動生成されたゴチャゴチャのファイルから必要な部分だけを残し、C++14でのプログラミング向けの要素を追加します。

cmake_minimum_required(VERSION 2.8.3)
project(six_point_two_eight)

find_package(catkin REQUIRED COMPONENTS
  roscpp
)

# find_package(Boost REQUIRED COMPONENTS system)

# add_message_files(
#   FILES
#   Message1.msg
#   Message2.msg
# )

# add_service_files(
#   FILES
#   Service1.srv
#   Service2.srv
# )

# add_action_files(
#   FILES
#   Action1.action
#   Action2.action
# )

# generate_messages(
#   DEPENDENCIES
#   std_msgs  # Or other packages containing msgs
# )

catkin_package(
#  CATKIN_DEPENDS                          # roscppを削除
)

add_compile_options(                       # 追加
  "-std=c++14"                             # 追加
)                                          # 追加

include_directories(
  include                                  # 追加
  ${catkin_INCLUDE_DIRS}
)

add_library(six_point_two_eight
  src/six_point_two_eight.cpp              # ${PROJECT_NAME}/を削除
)

# add_dependencies(six_point_two_eight ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(six_point_two_eight
  ${catkin_LIBRARIES}
)

add_compile_optionsで、C++14モードでコンパイルするように指示しています。あと、catkin_create_pkgはincludeディレクトリを作ったくせに、catkin_create_pkgが生成したCMakeLists.txtのままだと、インクルード対象のディレクトリになりません……。仕方がないですから、include_directoriesで追加しました。

g++ 4.9

最後に、もう一つだけ準備作業を。ROS Indigoの対象プラットフォームであるUbuntu 14.10のg++のバージョンは4.8で、バージョン4.8だとC++11までしかサポートしないんです……。

なので、以下のコマンドを実行して、g++ 4.9をセットアップしておいてください。

$ sudo add-apt-repository ppa:ubuntu-toolchain-r/test
$ sudo apt-get update
$ sudo apt-get install gcc-4.9 g++-4.9
$ sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-4.9 60 --slave /usr/bin/g++ g++ /usr/bin/g++-4.9
$ sudo update-alternatives --config gcc

Nodelet

やりました。ついに、プログラミングです。

*.hに書く？　*.cppに書く？

回りくどくて申し訳ないのですけど、古き良き時代の話から。C++が生まれる前、C言語の時代の物事は単純で、関数の宣言は*.h、関数の実装は*.cに書くと決まっていました。そして、C++が生まれ、クラス宣言を*.hに書くようになったところで、この美しい原則は壊れました。クラスのメンバー関数はC言語の関数よりも小さな場合があり、通常の関数呼び出しだとプログラムが遅くなるからインライン展開が必要で、その場合の実装は*.hに書くことになっちゃったんですよ。*.hに、宣言に加えて実装（の一部）を書くわけです。その後、ジェネリック・プログラミング（テンプレート）が混乱に拍車をかけました。ジェネリック・プログラミングはコンパイル時にコードを生成する技術で、コード生成には生成元のコードが必要です。コンパイル時には*.hしか参照できませんから、生成元コードは*.hに書くしかない……。というわけで、実装のほとんどが*.hにあるコードが増えてきました。たとえば、線形代数ライブラリのEigenでは、実装のすべてが*.hに入っています。*.hには、それが宣言であれ実装であれ、*.cppで共有しなければならないものは何でも書くことになりました。

では我々も*.hにコードを書けばよいのかというと、ROSの場合はちょっと微妙です。*.hには複数の*.cppで共有する内容を書くべきなのですけれど、でもROSのノードはROS通信で呼び出されます。だから、C++の仕組みでの呼び出しは不要なわけ。だからクラス宣言を*.hで共有する必要はないわけで、だったら全部を*.cppに書いても問題ないはず。折衷案として、インライン展開するかどうかはコンパイラの判断に任せることにして、クラス宣言は*.hに、メンバー関数の実装は*.cppに書くという手もあるのですけれど、それだとジェネック・プログラミングが使えません。ノード間で共有するユーティリティーではテンプレートを使いたいわけで、その場合だけ*.hに実装があるのではコードの閲覧性が悪くなってしまいます。

と、いろいろ考えても明らかな正解はなさそうだったので、実利だけを考え、本稿ではコード量が少なくて閲覧性が高い*.hに実装を書く方式を採用しました。

package.xmlとCMakeLists.txtにnodeletを追加

さて、我々はこれからNodeletを作成するわけですが、そのNodeletは、nodeletパッケージが提供する機能です。でも、我々のsix_point_two_eightパッケージが参照しているのはroscppパッケージだけ。このままではNodeletを作れませんので、nodeletパッケージを参照するようにpackage.xmlとCMakeLists.txtを編集しましょう6。

package.xml

<build_depend>にビルド時に必要なパッケージを、<run_depend>に実行時に必要なパッケージを指定します。nodeletパッケージはビルドでも実行でも必要ですから、両方を記述してください。

<?xml version="1.0"?>
<package>
  <!-- 略 -->
  
  <buildtool_depend>catkin</buildtool_depend>
  
  <build_depend>roscpp</build_depend>
  <build_depend>nodelet</build_depend>  <!-- 追加 -->
  
  <run_depend>roscpp</run_depend>
  <run_depend>nodelet</run_depend>      <!-- 追加 -->
  
  <!-- 略 -->
</package>

CMakeLists.txt

find_packageにnodeletを追加するだけです。

cmake_minimum_required(VERSION 2.8.3)
project(six_point_two_eight)

find_package(catkin REQUIRED COMPONENTS
  nodelet  # 追加
  roscpp
)

# 略

make_world_models.h

初めてのROSプログラミングですから、Hello, world!をやりましょう。include/six_point_two_eight/make_world_models.hを作成し、以下のコードをタイプしてください。

#pragma once

#include <nodelet/nodelet.h>
#include <ros/ros.h>

namespace six_point_two_eight {
  class MakeWorldModels : public nodelet::Nodelet {
  public:
    void onInit() {
      ROS_INFO_STREAM("Hello, world!");
    }
  };
}

ROS Style Guideでは、ファイル名はunder_scoredとなっています。なので、MakeWorldModelsというクラスのファイル名は、make_world_models.hになりました（本稿のコードの「{」の位置はスタイル・ガイドに準拠していませんけど、個人的に譲れないところなのでご容赦ください。皆様は、スタイル・ガイド準拠でコードを書いてください）。

Nodeletを作る時は、nodelet::Nodeletを継承しなければなりません。あと、初期化処理はonInit()に書きます。今回は、ROSのログ出力であるROS_INFO_STREAMを使用して「Hello, world!」という文字列を出力することにしました。Nodeletのドキュメントを見るとNODELET_INFO_STREAMというNodelet用のログ出力マクロが出てきますけど、Nodeletの外側で使用できなくて不便なので、本稿では使用しません。

そうそう、MakeWorldModelsというクラス名は「ロボットを動かしてみる」という内容と合っていませんが、これは、次の章の「対象物の周囲360°からの3D点群を作成する」につなげたいからなのでご容赦ください。

six_point_two_eight.cpp

*.hをインクルードする*.cppも作成しましょう。NodeletのクラスをエクスポートするにはPLUGINLIB_EXPORT_CLASSというマクロを呼び出しておく必要があって、ドキュメントにはそのマクロのヘッダー・ファイル（pluginlib/class_list_macros.h）は*.cppでインクルードしろと書いてあります。*.cppの記述は単純でクラス単位に分割する必要はなさそうですから、全体で一つ、パッケージ名と同じファイル名にしましょう。

というわけで、src/six_point_two_eight.cppに、以下のコードをタイプしてください。

#include <pluginlib/class_list_macros.h>
#include "six_point_two_eight/make_world_models.h"

PLUGINLIB_EXPORT_CLASS(six_point_two_eight::MakeWorldModels, nodelet::Nodelet)

コードを作成したら、ビルドです。catkinのビルドは、catkin_ws直下で実行します（ワーク・スペース全体のビルドにしないと、パッケージの整合性をとれませんから。catkin_ws\src\six_point_two_eightでビルドを実行しないように、気をつけてください）。ビルドのためのコマンドは、catkin_makeです。

$ cd <catkin_ws>
$ catkin_make
Base path: /home/ryo/Documents/Projects/catkin_ws
Source space: /home/ryo/Documents/Projects/catkin_ws/src
Build space: /home/ryo/Documents/Projects/catkin_ws/build
Devel space: /home/ryo/Documents/Projects/catkin_ws/devel
Install space: /home/ryo/Documents/Projects/catkin_ws/install
####
#### Running command: "make cmake_check_build_system" in "/home/ryo/Documents/Projects/catkin_ws/build"
####
-- Using CATKIN_DEVEL_PREFIX: /home/ryo/Documents/Projects/catkin_ws/devel
（後略）

catkin_makeの出力には進捗が％で表示されるのですけれど、それが100%になってエラーが表示されてなければ、ビルド成功です。

six_point_two_eight.xmlと、もう一度package.xml

作成したNodeletには、ROS向けの名前をつけてあげなければなりません。まずは、six_point_two_eight.xmlを作成し、以下のコードをタイプしてください。

<library path="lib/libsix_point_two_eight">
  <class name="six_point_two_eight/make_world_models" type="six_point_two_eight::MakeWorldModels" base_class_type="nodelet::Nodelet"/>
</library>

次に、作成したXMLファイルをROSに認識させるために、package.xmlの<export>に1行追記してください。

<?xml version="1.0"?>
<package>
  <!-- 略 -->
  <export>
    <nodelet plugin="${prefix}/six_point_two_eight.xml" />  <!-- 追加 -->
  </export>
</package>

make_world_models.launch

さて、Nodeletはnodeletパッケージのnodeletコマンドを通じて起動するのですけれど、これが実に面倒くさい。だから、前述したroslaunchで起動することにしましょう。launch/make_world_models.launchを作成して、以下をタイプしてください。

<launch>
  <node pkg="nodelet" type="nodelet" name="six_point_two_eight_nodelet_manager" args="manager" output="screen"/>
  <node pkg="nodelet" type="nodelet" name="make_world_models" args="load six_point_two_eight/make_world_models six_point_two_eight_nodelet_manager" output="screen"/>
</launch>

2行目で、NodeletManagerをプロセスとして起動しています。このNodeletManagerの中に、複数のNodeletを起動するわけです（今回は一つだけですけど）。output="screen"と指定しているのは、ログを画面に出力させたいから。これを書いておかないと、何か問題が起きても画面に何も表示されなくて、開発時にかなり混乱します。

3行目が、作成したNodeletの起動です。args属性のloadの次が起動するNodeletの名前で、その次がNodeletManagerの名前です。今回はNodeletManagerに2行目で起動したsix_point_two_eight_nodelet_managerを指定しましたが、ロボットのドライバ等が作成したNodeletManagerを指定することも可能です。たとえば、TurtleBotの深度センサーのドライバが動作しているcamera/camera_nodelet_managerというNodeletManagerとか。six_point_two_eight_nodelet_managerの代わりにcamera/camera_nodelet_managerを指定すれば、深度センサーからデータを取得する速度が、ネットワーク通信が減る分だけ向上するでしょう。ただし、作成したプログラムの不具合でNodeletManagerが異常終了して深度センサーのドライバまで止まってしまうという危険性もありますから、開発時は専用のNodeletManagerの使用を推奨します。

これですべての作業は完了しましたので、さっそく、我々のNodeletを起動してみましょう。

$ roslaunch six_point_two_eight make_world_models.launch
... logging to /home/ryo/.ros/log/60de7594-df68-11e5-b06d-3b9332081cf6/roslaunch-ryo-T550-17006.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://ryo-T550:33789/

SUMMARY
========

PARAMETERS
 * /rosdistro: indigo
 * /rosversion: 1.11.16

NODES
  /
    make_world_models (nodelet/nodelet)
    six_point_two_eight_nodelet_manager (nodelet/nodelet)

auto-starting new master
process[master]: started with pid [17018]
ROS_MASTER_URI=http://ryo-T550:11311

setting /run_id to 60de7594-df68-11e5-b06d-3b9332081cf6
process[rosout-1]: started with pid [17031]
started core service [/rosout]
process[six_point_two_eight_nodelet_manager-2]: started with pid [17034]
process[make_world_models-3]: started with pid [17044]
[ INFO] [1456807510.442436453]: Loading nodelet /make_world_models of type six_point_two_eight/make_world_models to manager six_point_two_eight_nodelet_manager with the following remappings:
[ INFO] [1456807510.444383031]: waitForService: Service [/six_point_two_eight_nodelet_manager/load_nodelet] has not been advertised, waiting...
[ INFO] [1456807510.482190090]: Initializing nodelet with 4 worker threads.
[ INFO] [1456807510.486318180]: waitForService: Service [/six_point_two_eight_nodelet_manager/load_nodelet] is now available.
[ INFO] [1456807510.538166303]: Hello, world!

うん、最後の行に、「Hello, world!」が出力されました……。

Publisher

おいおい、「Hello, world!」と画面に出すだけって、ROS関係ないじゃん！　ごめんなさい、おっしゃる通りです。もう少し高度なこと、TurtleBotを前に進ませる処理を書いてみましょう。

トピックとメッセージ

さて、ロボットに命令を出すにはどうすればよいのか？　前述したように、適切なトピックに適切なメッセージをpublishすればよい。でも、ロボットを移動させる場合は、具体的にどのトピックとメッセージを使えばいいの？

それを調べるツールとして、ROSはrostopicとrosmsgを提供しています。試してみましょう。まずは、ターミナルを開いてTurtleBotのドライバを起動してください。

$ roslaunch turtlebot_bringup minimal.launch
... logging to /home/ryo/.ros/log/b43068d0-df70-11e5-b152-630afb514811/roslaunch-ryo-T550-22396.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://ryo-T550:45177/

SUMMARY
========

PARAMETERS
 * /app_manager/auto_rapp_installation: False

いろいろメッセージが出て、中にはWarningと書かれていたりもしますけど、エラーが出ない限りは大丈夫です。

もう一つターミナルを開いて、rostopic listしてみてください。

$ rostopic list
ryo@ryo-T550:~/Documents/Projects/catkin_ws$ rostopic list
/capability_server/bonds
/capability_server/events
/cmd_vel_mux/active
/cmd_vel_mux/input/navi
/cmd_vel_mux/input/safety_controller
/cmd_vel_mux/input/teleop
/cmd_vel_mux/parameter_descriptions
/cmd_vel_mux/parameter_updates
/diagnostics
/diagnostics_agg
/diagnostics_toplevel_state
/gateway/force_update
/gateway/gateway_info
/info
/interactions/interactive_clients
/interactions/pairing
/joint_states
/laptop_charge
/mobile_base/commands/controller_info
/mobile_base/commands/digital_output
/mobile_base/commands/external_power
/mobile_base/commands/led1
/mobile_base/commands/led2
/mobile_base/commands/motor_power
/mobile_base/commands/reset_odometry
/mobile_base/commands/sound
/mobile_base/commands/velocity
/mobile_base/controller_info
/mobile_base/debug/raw_control_command
/mobile_base/debug/raw_data_command
/mobile_base/debug/raw_data_stream
/mobile_base/events/bumper
/mobile_base/events/button
/mobile_base/events/cliff
/mobile_base/events/digital_input
/mobile_base/events/power_system
/mobile_base/events/robot_state
/mobile_base/events/wheel_drop
/mobile_base/sensors/bumper_pointcloud
/mobile_base/sensors/core
/mobile_base/sensors/dock_ir
/mobile_base/sensors/imu_data
/mobile_base/sensors/imu_data_raw
/mobile_base/version_info
/mobile_base_nodelet_manager/bond
/odom
/rosout
/rosout_agg
/tf
/tf_static
/turtlebot/incompatible_rapp_list
/turtlebot/rapp_list
/turtlebot/status
/zeroconf/lost_connections
/zeroconf/new_connections

これが、TurtleBotのドライバ起動後にROS上に存在するトピックの一覧です。この中から、適切なトピックを選べばよいというわけ……なんですけど、TurtleBotのドキュメントを見てもトピックの説明は見つかりませんでした。しょうがないので、rosmsgも活用して、アタリをつけていきます。

$ rostopic info /mobile_base/commands/velocity
Type: geometry_msgs/Twist

Publishers: 
 * /mobile_base_nodelet_manager (http://ryo-T550:58177/)

Subscribers: 
 * /mobile_base_nodelet_manager (http://ryo-T550:58177/)

$ rosmsg show geometry_msgs/Twist
geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

rostopic info <トピック名>で、トピックの情報を表示できます。表示内容で重要なのは1行目のType:で、これで/mobile_base/commands/velocityトピックにはgeometry_msgs/Twistをpublish/subscribeできるということが分かります。で、rosmsg show <メッセージ名>でメッセージの型情報を表示できるので、なんとなく推測ができるというわけ。上の情報から、/mobile_base/commands/velocityトピックがたぶん移動で、geometry_msgs/Twistのlinearが直線移動でangularが回転なんだなぁと推測できます。カン頼みのでたらめな方法ですけど、試して駄目なら別のトピックを調べればいいだけですし、ROSアプリケーションに慣れるとカンが冴えてくるので、それほど大きな問題にはなりません。

別のやり方としては、同じようなことをしているTurtleBot向けROSアプリケーションのコードを調べるという手があります。まずは、ROS.orgのBrowse Softwareでindigoとpackageを選択してテキスト・ボックスに「turtlebot」と入力し、searchボタンをクリックしてください。

Browse Software

検索結果の中から、アプリケーションっぽい感じがするturtlebot_appsをクリック。

Browse Software result

今はソース・コードを見たいので、Source:の後ろのhttps://github.com/turtlebot/turtlebot_apps.gitをクリック。

TurtleBot Apps

移動に関係がありそうなturtlebot_follower、src、follower.cppと開いて、この後に述べる知識を活用してコードを読む。そうすると、多分geometry_msgs::Twistが移動用のメッセージなんだろうなぁと推測できます。

TurtleBot Apps
follower.cpp

もし推測したトピックとメッセージに自信がないなら、rostopic pub <トピック名> <メッセージ名> <YAML形式のデータ>を使って、トピックにメッセージをpublishすることもできます。プログラムを書く前に、試してみましょう。

$ rostopic pub /mobile_base/commands/velocity geometry_msgs/Twist '{linear: {x: 0.1}}'

はい、TurtleBotが少し前に動きました。これで、/mobile_base/commands/velocityトピックとgeometry_msgs/TwistメッセージでTurtleBotを動かせることが分かりました。

座標系と単位

プログラミングに入る前に、ROSの座標系について述べさせてください。先ほどのrostopic pubでlinear.xに0.1を指定すると前に進むってのを理解するには、座標系と単位の知識が必要ですもんね。

ROSの座標系は、X軸が前でY軸が左、Z軸が上です。また、X軸を赤、Y軸を緑、Z軸を青で描きます。だから、linear.xにプラスの数値を入れると前に進むというわけ。回転の場合は、座標軸の正の方向を見た場合の時計回り、負の方向に見ると反時計回りになります。だから、Z軸の回転は、ロボットを上から見ると反時計回り。なので、rostopic pub /mobile_base/commands/velocity geometry_msgs/Twist '{angular: {z: 0.5}}'とすると、少しだけ反時計回りします。

Coordinate System

あと、距離の単位はメートル、時間の単位は秒、角度の単位はラジアン、周波数はHzです。linear.x = 0.1は、秒速0.1mで前進しろ、angular.z = 0.5は秒0.5ラジアンで反時計回りに回転しろという意味になります。

package.xmlとCMakeLists.txt

さて、移動のメッセージはgeometry_msgs/Twistであることは分かったわけですけど、このメッセージを使うにはpackage.xmlとCMakeLists.txtの修正が必要です。ROSのメッセージ名は<パッケージ名>/<クラス名>に分解できますので、それぞれにgeometry_msgsを追加しましょう。

package.xml

<?xml version="1.0"?>
<package>
  <!-- 略 -->
  
  <buildtool_depend>catkin</buildtool_depend>
  
  <build_depend>geometry_msgs</build_depend>  <!-- 追加 -->
  <build_depend>nodelet</build_depend>
  <build_depend>roscpp</build_depend>
  
  <run_depend>geometry_msgs</run_depend>      <!-- 追加 -->
  <run_depend>roscpp</run_depend>
  <run_depend>nodelet</run_depend>

  <!-- 略 -->
</package>

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(six_point_two_eight)

find_package(catkin REQUIRED COMPONENTS
  geometry_msgs  # 追加
  nodelet
  roscpp
)

# find_package(Boost REQUIRED COMPONENTS system)

# 略

make_world_models.hpp

前準備が完了しましたので、プログラミングしましょう。ロボットを前に進ませるコードを追加したinclude/six_point_two_eight/make_world_models.hは、以下の通り。

#pragma once

#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <thread>

#include <geometry_msgs/Twist.h>

namespace six_point_two_eight {
  inline auto createTwistMsg(float linear_x, float angular_z) {
    geometry_msgs::Twist msg;
    msg.linear.x = linear_x;
    msg.angular.z = angular_z;
    
    return msg;
  }
  
  class MakeWorldModels : public nodelet::Nodelet {
  private:
    ros::Publisher velocity_publisher_;
    std::thread working_thread_;
    
  public:
    void onInit() {
      // Publisherを生成します。
      velocity_publisher_ = getNodeHandle().advertise<geometry_msgs::Twist>("mobile_base/commands/velocity", 1);

      // onInitの中でループするとNodeletManagerの処理が止まってしまうので、別スレッドで実行します。
      working_thread_ = std::thread(
        [&]() {
          ros::Rate rate(10);  // 周波数は10Hz（1秒に10回ループします）。
          while (ros::ok()) {  // ROSがシャットダウンされるまで、無限ループします。
            velocity_publisher_.publish(createTwistMsg(0.1, 0.0));  // メッセージを生成して、publishします。
            rate.sleep();  // 適切な周波数になるようにスリープ。
          }
        });
    }
  };
}

geometry_msgs/Twistを使えるようにするために、#include <geometry_msgs/Twist.h>しています。あと、ROSのメッセージには気の利いたコンストラクタが無くて使いづらいので、createTwistMsg()というメッセージ生成用の関数を作りました（TurtleBotは前後進とZ軸の回転しかできないので、linear.xとangular.zのみを設定すれば十分）。メンバー関数ではなくて普通の関数なので、inlineを付加しています。C++14だと、関数の戻り値の型をautoにしておくと型推論してくれてとても便利ですな（型が無いのとは違います。静的な型チェックが実施されますから、コードに型間違いがあればコンパイル・エラーになって安心でもあります）。

publishは、ros::Publisherを使用して実施します。ros::Publisherは、ros::NodeHandleクラスのadvertise<メッセージの型>(トピック名, キューのサイズ)メンバー関数で作成できます。このros::NodeHandleクラスってのはROSノードへのハンドルで、ノードに関する操作はこのNodeHandleを通じて実施します（Nodeletはノードを名乗っていますけど、ROSノードを操作するためのメンバー関数は提供しません……）。で、Nodeletの中でros::NodeHandleを取得するメンバー関数が、上のコードで使用しているgetNodeHandle()なわけです。

getNodeHandle()には、対になるgetMTNodeHandle()というマルチ・スレッド対応バージョンもあるのですけれど、その場合はスレッドの排他制御を自前でプログラミングしなければなりません。getNodeHandle()ならROSからの呼び出しが直列化され、排他制御が不要になってとても楽ちんで安心です。あと、getPrivateNodeHandle()とgetMTPrivateNodeHandle()というPrivate付きのバージョンもあるのですけど、Private付きだと名前解決にノード名が追加されてしまいます（mobile_base/commands/velocityが/make_world_models/mobile_base/commands/velocityと解釈されちゃう7）。というわけで、通常はgetNodeHandle()を使ってください。

あとは、作成したros::Publisherのインスタンスが消えてしまわないよう、メンバー変数のvelocity_publisher_に保持しておきます。

onInit()の後半。別スレッドを起こしてループしているところ。なんでこんな面倒なことをしているかを理解していただくために、先ほどのrostopic echoの時のロボットの動きを思い出してください。あの時、ロボットは少しだけ動いてすぐに止まってしまいましたよね？　これは、ロボットは一定周期で指示を受け取る仕組みになっているためです。動けと言われたので1周期分動いて、次の周期には命令が来ていないから動きを止めちゃったってわけ。というわけで、ロボットを動かし続けるためにはループが必要です。

で、getNodeHandle()のところで少し述べたように、Nodeletはスレッドの排他制御が不要になる仕組みを用意しています。そのためには、同じリソースを扱うスレッドを並行に動作させない（直列化して順番に実行させる）ことが有効です。NodeletでgetNodeHandle()を使うと、異なるノードの処理は並行で、同じノードの処理は直列で動作するようにになります。ただし、onInit()はどうやら全ノードで直列らしくて、onInit()の中でループすると他のノードの初期化が始まらなくなってしまいます。というわけで、ループのような時間がかかる処理を実行する場合は、上のコードのようにstd::threadを使用して別スレッドを起こしてあげてください。

最後。std::threadの引数の[&]() { ... }の部分は、C++14のラムダ式です。関数をその場で生成して、std::threadのコンストラクタに渡します。ラムダ式の最初の[]には処理の中で使用するラムダ式の外側の変数を書くのですけど、ここに[&]と書いておくと外側の変数すべてを使用できるようになってとても便利です（だから、ラムダ式の中でvelocity_publisher_が使えています）。

実行する

TurtleBotのノードにメッセージをpublishするプログラムを作成したわけですから、まずはTurtleBotのノードを起動します。ターミナルを開いて、以下のコマンドを実行してください（「トピックとメッセージ」のところで起動済みの場合は、コマンドの実行は不要です）。

$ roslaunch turtlebot_bringup minimal.launch

その上で、別のターミナルでmake_world_models.launchをroslaunchしてください。

$ roslaunch six_point_two_eight make_world_models.launch

これで、TurtleBotが前に動き出すはずです。今回作成したプログラムはひたすらまっすぐに進むだけですから、TurtleBotが壁にぶつかる前にCtrl-cして止めてあげてください。

Timer

まっすぐ進むだけなら、ゼンマイ仕掛けのおもちゃでもできちゃう……。せっかくのROSなのだから、もっと制御したい。というわけで、一定時間後に止まるようにしてみましょう。そのために、ros::Timerを使用します。

make_world_models.hpp

ros::Timerは簡単に使用できますので、いきなりコードで。

#pragma once

#include <nodelet/nodelet.h>
#include <ros/ros.h>

#include <geometry_msgs/Twist.h>

namespace six_point_two_eight {
  inline auto createTwistMsg(float linear_x, float angular_z) {
    geometry_msgs::Twist msg;
    msg.linear.x = linear_x;
    msg.angular.z = angular_z;
    
    return msg;
  }
  
  class MakeWorldModels : public nodelet::Nodelet {
  private:
    ros::Publisher velocity_publisher_;
    ros::Timer timer_1_;
    ros::Timer timer_2_;
    
  public:
    void onInit() {
      // Publisherを生成します。
      velocity_publisher_ = getNodeHandle().advertise<geometry_msgs::Twist>("mobile_base/commands/velocity", 1);

      // ループの代わりにros::Timerを使用します。
      timer_1_ = getNodeHandle().createTimer(
        ros::Duration(0.1),  // 0.1秒に一回＝10Hz。
        [&](const auto& message) {
          velocity_publisher_.publish(createTwistMsg(0.1, 0.0));
        });

      // 5秒経ったら、処理を止めます。
      timer_2_ = getNodeHandle().createTimer(
        ros::Duration(5.0),
        [&](const auto& message) {
          timer_1_.stop();
          timer_2_.stop();
        });
    }
  };
}

ros::Timerは、引数で指定した周期で、引数で指定した処理を呼び出してくれます。ROSでの時刻はros::Timeで時間はros::Durationなので、ros::Timerを作成するcreateTimer()の第一引数はros::Durationです。第二引数は実行する処理で、関数とかクラス・メソッドとかファンクターとか、あとラムダ式が使えます。C++14のラムダ式は、引数の型にもautoが使えてとても便利です。constと&をつけているのは、&だと参照渡しになるのでインスタンスのコピーが不要になって高速化でき、constを付けておけば参照渡しであっても呼び出し側の値が変更されないため値渡しの場合と同じコードを書けるから。最近のC++のライブラリの引数はconst type& varばっかりなわけで、他の書き方だと変な意図があるんじゃないかと混乱させちゃうしね。

今回のコードでは、ros::Publisherの知識だけで書いた先ほどのコードのスレッドとループをros::Timerに置き換えて、timer_1_に格納しています。この書き方の方が楽ちんですよね。あと、移動を停止するのは、移動するようにpublishするタイマーを止めればよいわけですから、5秒後（ros::Duration(5.0)）にtimer_1_.stop()するros::Timerを設定してtimer_2_に格納しています。あと、timer_2_が5秒周期で何回も処理を実施するのは無駄ですから、timer_2_のラムダ式の中ではtimer_2_.stop()も実行しておきます。

はい、お疲れ様。これで、TurtleBotが動いて止まるプログラムが完成しました！　……まぁ、ゼンマイ仕掛けの安物のおもちゃでも、動いて止まるけどな。

ロボットの周囲360°の3D点群を作成する

単純な動きだけじゃツマラナイ！　センサーを使いたい！　了解しました。TurtleBotには深度センサーが付いているので、それを活用して外界データを作成してみましょう。プログラムから、以下の画像のようなデータを作ります。以下の画像のデータはオフィスの私の席の近くの3D点群で、左上で腕をブラブラさせてだらけまくっているのが私になります。もちろん、グリグリ動きます。

周囲360°の3D点群

PointCloud2

TurtleBotには、Microsft社のKinectかASUS社のXtion PRO LIVEがついています。これらは深度センサーと呼ばれるデバイスで、外界の3D点群を作成してくれます（Kinectというと骨格のイメージがありますけど、骨格はこの3D点群を解析して作成しています）。

深度センサーを扱うプログラム開発時の、TurtleBotとの接続方法

TurtleBot Installationでは、PCを2台用意して、1台をTurtleBotの上に、残り1台を手元に置くようになっています。でも、深度センサーのデータは大きいので、この方式だとフレームレートが低く（私の環境は数秒に1コマ！）なってしまうんですよ。本番環境では深度センサーのデータ処理はTurtleBot上のPCで実行するので問題ないでしょうけど、このままだと開発は大変です。

なので、今回は手元のPCにTurtleBotをつなぐことにしましょう。長めのUSB延長ケーブルを用意して、手元のPCにTurtleBotをつなげてください。

RViz

では、深度センサーのデータがどんなものか、試してみましょう。ROSにはRVizという可視化ツールがあって、それで深度センサーのデータを表示できるんです。ターミナルを3枚開いて、以下のコマンドを実行してください。

$ roslaunch turtlebot_bringup minimal.launch
$ roslaunch turtlebot_bringup 3dsensor.launch
$ roslaunch turtlebot_rviz_launchers view_model.launch

これで、RVizの画面が表示されるはずです。画面が表示されたら左下の[Add]ボタンを押して、表示されたダイアログの[By display type]タブの[PointCloud2]アイコンを選択し、[OK]ボタンを押してください。RVizの画面左側の[Displays]パネルに[PointCloud2]アイコンが追加されるはずです。なお、ここまでGUI上で実施した作業は、プログラミングでのメッセージの型の指定に相当します。

RVizへのメッセージの追加

ROSではメッセージとトピックの両方が必要なのは前の章でやりましたから、メッセージに続いてトピックを設定しましょう。[PointCloud2]アイコンの下の[Topic]ドロップ・ダウン・リストを開いて、「/camera/depth_registered/points」（シミュレーターの場合は「/camera/depth/points」）を選択してください。これで、深度センサーのデータが画面に表示されるはずです。マウスの左ボタンのドラッグで回転、シフト・キーを押しながらの左ドラッグで平行移動、右ドラッグで拡大/縮小できますから、グリグリ回して楽しんでみてください。

RVizでのトピック選択

グリグリ回している途中のキャプチャなので分かりづらいですけれど、3Dのデータができています。

3D点群

なお、実機の場合でトピックを「/camera/depth/points」に変更すると、カメラからの画像を合成していない（色が着いていない）点群が表示されます。このように、トピックやメッセージの内容を調べるためにもRVizは使えますから、いろいろと活用してみてください。

PointCloud2

さて、先ほどRVizのメッセージ型として選択したのは、PointCloud2でした。ポイント・クラウドの日本語訳は点群（数学の点群と字は同じだけど意味は違う）で、点の集合です。RVizの[PointCloud2]アイコン以下の[Style]を「Points」、[Size (Pixels)]を「1」にして点群部分を拡大してみると、点の集合であることが分かります。なお、PointCloud2の「2」は、2次元じゃなくてバージョン2の「2」です。以前のPointCloudメッセージは拡張性が不足していたみたいで、新しくPointCloud2が定義されました。

Wikipediaによれば、点群から面を計算してポリゴンにしたり、NURB（Non-uniform rational B-spline）で曲面を出したりすることもできるようです。うん、点群ってなんだか便利そう。ただ表示するだけでも、ちょっと面白いですしね。そうだ、TurtleBotにその場でグルッと回転させて、周囲360°の点群を取得して表示してグリグリしたら楽しいんじゃね？

Subscriber

というわけで、TurtleBotの周囲360°の点群を生成するROSプログラムを作りましょう。そのために、まずは深度センサーからデータをもらわないと。

いつものpackage.xmlとCMakeLists.txt

PointCloud2メッセージを扱うので、いつものとおりに参照するパッケージを追加しなければなりません。でも、PointCloud2のパッケージって、どうやって調べればよいのでしょうか？　答え。rosmsg show PointCloud2で調べられます。

ryo@ryo-T550:~$ rosmsg show PointCloud2
[sensor_msgs/PointCloud2]:
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
uint32 height
uint32 width
sensor_msgs/PointField[] fields
  uint8 INT8=1
  uint8 UINT8=2
  uint8 INT16=3
  uint8 UINT16=4
  uint8 INT32=5
  uint8 UINT32=6
  uint8 FLOAT32=7
  uint8 FLOAT64=8
  string name
  uint32 offset
  uint8 datatype
  uint32 count
bool is_bigendian
uint32 point_step
uint32 row_step
uint8[] data
bool is_dense

というわけで、sensor_msgsを追加しましょう。

package.xml

<?xml version="1.0"?>
<package>
  <!-- 略 -->
  
  <buildtool_depend>catkin</buildtool_depend>
  
  <build_depend>geometry_msgs</build_depend>
  <build_depend>nodelet</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>sensor_msgs</build_depend>  <!-- 追加 -->
  
  <run_depend>geometry_msgs</run_depend>
  <run_depend>roscpp</run_depend>
  <run_depend>nodelet</run_depend>
  <run_depend>sensor_msgs</run_depend>      <!-- 追加 -->

  <!-- 略 -->
</package>

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(six_point_two_eight)

find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  nodelet
  roscpp
  sensor_msgs  <!-- 追加 -->
)

# 略

roslaunchで、トピックをリマップ。あと、includeも

先ほどRVizで点群を見た時、TurtleBotの実機とシミュレーターで、設定するトピックが異なっていました。実機は/camera/depth_registered/pointsで、シミュレーターでは/camera/depth/pointsでしたもんね。ということは、トピックだけが異なっていて他は同じプログラムを2つ作らなければならない？

そんな保守性ダダ下がりの馬鹿な話はないわけで、roslaunchに少し記述を追加するだけでトピックをリマップできます。

実機用

<launch>
  <node pkg="nodelet" type="nodelet" name="six_point_two_eight_nodelet_manager" args="manager" output="screen"/>
  <node pkg="nodelet" type="nodelet" name="make_world_models" args="load six_point_two_eight/make_world_models six_point_two_eight_nodelet_manager" output="screen">
    <remap from="points" to="camera/depth_registered/points"/>  <!-- 追加 -->
  </node>
</launch>

シミュレーター用

<launch>
  <node pkg="nodelet" type="nodelet" name="six_point_two_eight_nodelet_manager" args="manager" output="screen"/>
  <node pkg="nodelet" type="nodelet" name="make_world_models" args="load six_point_two_eight/make_world_models six_point_two_eight_nodelet_manager" output="screen">
    <remap from="points" to="camera/depth/points"/>  <!-- 変更 -->
  </node>
</launch>

あれ？　トピックだけが異なっていて他は同じlaunchファイルを2つ作るのは、保守性ダダ下がりでやっぱり馬鹿らしいので状況は悪いまま？　というわけで、roslaunchのドキュメントにのっている、<arg>と<include>を使ってみます。ついでに、mobile_base/commands/velocityも<remap>しちゃいましょう。

実機用（launch/make_world_models.launch）

<launch>
  <arg name="velocity" default="mobile_base/commands/velocity"/>  <!-- argを宣言 -->
  <arg name="points" default="camera/depth_registered/points"/>   <!-- argを宣言 -->
  
  <node pkg="nodelet" type="nodelet" name="six_point_two_eight_nodelet_manager" args="manager" output="screen"/>
  <node pkg="nodelet" type="nodelet" name="make_world_models" args="load six_point_two_eight/make_world_models six_point_two_eight_nodelet_manager" output="screen">
    <remap from="velocity" to="$(arg velocity)"/>  <!-- argを使用 -->
    <remap from="points" to="$(arg points)"/>      <!-- argを使用 -->
  </node>
</launch>

シミュレーター用（launch/make_world_models_in_gazebo.launch）

<launch>
  <include file="$(find six_point_two_eight)/launch/make_world_models.launch">
    <arg name="points" value="camera/depth/points"/>  <!-- argの値を上書きします。 -->
  </include>
</launch>

はい、これで重複がなくなりました。<include>と<arg>って便利ですね。ついでですから、turtlebot_bringup minimal.launchとturtlebot_bringup 3dsensor.launchを2回実行するのは面倒なので、<include>を使用してこの2つをまとめるlaunchファイルも作成しましょう。

TurtleBot起動用（launch/turtlebot_driver.launch）

<launch>
  <include file="$(find turtlebot_bringup)/launch/minimal.launch" />
  <include file="$(find turtlebot_bringup)/launch/3dsensor.launch" />
</launch>

make_world_models.h

以上で準備は完了。プログラミングに入りましょう。

#pragma once

#include <nodelet/nodelet.h>
#include <ros/ros.h>

#include <geometry_msgs/Twist.h>
#include <sensor_msgs/PointCloud2.h>  // 追加。

namespace six_point_two_eight {
  inline auto createTwistMsg(float linear_x, float angular_z) {
    geometry_msgs::Twist msg;
    msg.linear.x = linear_x;
    msg.angular.z = angular_z;
    
    return msg;
  }
  
  class MakeWorldModels : public nodelet::Nodelet {
  private:
    ros::Publisher velocity_publisher_;
    ros::Subscriber points_subscriber_;  // サブスクライバーを保持するメンバー変数。
    ros::Timer timer_1_;
    ros::Timer timer_2_;

  public:
    void onInit() {
      velocity_publisher_ = getNodeHandle().advertise<geometry_msgs::Twist>("velocity", 1);  // remapに合わせて、トピックを変更。

      // PointCloud2を取得するサブスクライバーを設定します。
      points_subscriber_ = getNodeHandle().subscribe<sensor_msgs::PointCloud2>(
        "points", 10,  // トピックはremapのfromの値。
        [&](sensor_msgs::PointCloud2ConstPtr message) {  // ラムダ式の展開のされかた次第では（具体的には、中で外側の変数や関数を呼ばない場合は）、const auto&は使えませんでした。
          ROS_INFO_STREAM("Got PointCloud2. Width: " << message->width);  // とりあえず、データを取得したことを表示。
        });

      timer_1_ = getNodeHandle().createTimer(
        ros::Duration(0.1),
        [&](const auto& message) {
          velocity_publisher_.publish(createTwistMsg(0.0, M_PI * 2 / 30));  // 前進ではなく、回転に変更。1周（M_PI * 2）を30秒で回る速度を指示します。
        });
      
      timer_2_ = getNodeHandle().createTimer(
        ros::Duration(45.0),  // 計算上は30秒でよいはずなのですけど、30秒では一周してくれませんでしたので、多めの値で。
        [&](const auto& message) {
          points_subscriber_.shutdown();  // ros::Subscriberの終了は、stop()ではなくてshutdown()です。
          timer_1_.stop();
          timer_2_.stop();
        });
    }
  };
}

subscribeは、ros::Subscriberを使用して実施します。getNodehandle()してros::NodeHandleを取得して、そのメンバー関数のsubscribe<メッセージの方>(トピック名、キューのサイズ、メッセージ取得時の処理)を呼び出してください。引数のトピック名はmake_world_models.launchの<remap>のfrom属性に合わせ、キューのサイズは処理中に次のデータがやってきても取りこぼさないよう、多めに10を指定してみました。

処理は、例によってラムダ式で指定します。C++のラムダ式はその内容によって展開のされ方が違う、かつ、しかもROS側での受け取り引数型のboost::functionは癖が強いみたいで、ラムダ式の引数の型をconst auto&にできる場合とできない場合がありました。どうやら、ラムダ式の中でキャプチャした変数や関数を呼び出しているのでファンクター相当品が生成される場合はconst auto&OKで、今回のコードのようにタダの無名の関数で表現できる場合は駄目みたいです。というわけで、今回はメッセージを受け取る処理の引数の型を明示しました。この型は、コンスタントでスマート・ポインターな型のエイリアスで、メッセージの型の後ろにConstPtrという文字列を付加した型になります（ros::NodeHandle.subscribe()のコードを見るとconst sensor_msgs::PointCloud2&を引数に使うバージョンもあるのですけれど、ラムダ式の場合は駄目でした）。処理の内容は、rosmsg show sensor_msgs/PointCloud2で見ても何を表現しているのか分からないデータばかりだったので、とりあえず、widthを表示するだけにしています。

とまぁ、これでセンサーのデータを読み込めるようになったわけですけど、それにしても、センサーのデータを「読み込む」処理なのに、データを渡された時に実行する処理を渡す形になっているのは、なぜなのでしょうか？　私は、「読み取りなんだから戻り値でいーじゃん」と感じました。

その答えは多分、ROSは様々な周波数で動作する大きめのユニットを統合するための環境だから。「様々な周波数」なのだから、前の章の最初の頃のTimerを使う前のプログラムのようなループを組んで、「一定の周波数」で動作する中でセンサーのデータを読み取る方式は採用できません。センサーやユニットがデータを送りたいタイミングでデータを取得できないとね。だから、処理を登録する形になっているんだと思います。ROS.orgに載っているプログラムはサンプル・プログラムなので単純な場合が多く、単純な処理なのでループで実現していてArduinoのコードと見た目そっくりでだったらArduinoみたいに読み取りでいいじゃんと混乱してしまうのですけれど、実際のアプリケーションを作成すれば、この方式が優れていることが分かると考えます。まぁ、使いづらくて腹がたつ場合も多いんですけど、次の章で紹介するactionlibとかラムダ式とスマート・ポインターの組み合わせとかで対処できるのでまぁいいかなぁと。

nodelet_topic_tools::NodeletThrottle

とはいえ、データを受け取って処理する側の都合も重要です。publish側の深度センサーが全力で送ってくるデータをすべて処理するのは、かなり大変ですもんね。私が使っているASUS Xtion PRO LIVEの読み取りは30fpsなんで、30秒かけてTurtleBotを一周させる先ほどのプログラムだと、点群の数が900個になっちゃう。なんとかして、深度センサーの周波数を下げたい。

そんな時は、nodelet_topic_toolsパッケージのNodeletThrottleが便利です。メッセージの流量制御を、パフォーマンスを落とさずにとても簡単に実現できます。さっそく使ってみましょう。

package.xmlとCMakeLists.txt

依存するパッケージに、nodelet_topic_toolsを追加してあげてください。これまで何度も見て飽きているでしょうから、コードは省略します。

include/six_point_two_eight/point_cloud_2_throttle.h

nodelet_topic_tools::NodeletThrottleクラスを継承して、throttle()メンバー関数を実装する……などという手間はかかりません。ただし、取り扱うメッセージの型だけは指定しなければなりませんので、usingしてテンプレート引数を補います。C++14（正しくはC++11以降）ではtypedefとusingの両方があって、usingの方が高機能です。nodelet_topic_toolsのドキュメントではtypedefを使っていますけど、そして、今回の使い方ではどっちを使っても変わらないのですけれど、コードを統一するためにusingしましょう。

#pragma once

#include <nodelet_topic_tools/nodelet_throttle.h>
#include <sensor_msgs/PointCloud2.h>

namespace six_point_two_eight {
  using PointCloud2Throttle = nodelet_topic_tools::NodeletThrottle<sensor_msgs::PointCloud2>;
}

src/six_point_two_eight.cpp

Nodeletと同様に、エクスポートします。

#include <pluginlib/class_list_macros.h>

#include "six_point_two_eight/make_world_models.h"
#include "six_point_two_eight/point_cloud_2_throttle.h"  // 追加。

PLUGINLIB_EXPORT_CLASS(six_point_two_eight::MakeWorldModels, nodelet::Nodelet)
PLUGINLIB_EXPORT_CLASS(six_point_two_eight::PointCloud2Throttle, nodelet::Nodelet)  // 追加。

six_point_two_eight.xml

やはりNodeletと同様に、ROSに名前を登録します。

<library path="lib/libsix_point_two_eight">
  <class name="six_point_two_eight/make_world_models" type="six_point_two_eight::MakeWorldModels" base_class_type="nodelet::Nodelet"/>
  <class name="six_point_two_eight/point_cloud_2_throttle" type="six_point_two_eight::PointCloud2Throttle" base_class_type="nodelet::Nodelet"/>  <!-- 追加 -->
</library>

launch/make_world_models.launch

起動もNodeletと一緒。topic_inから取得したメッセージを、<param name="update_rate">を通じてHzで設定した周波数でtopic_outに流します。

<launch>
  <arg name="velocity" default="mobile_base/commands/velocity"/>
  <arg name="points" default="camera/depth_registered/points"/>
  
  <node pkg="nodelet" type="nodelet" name="six_point_two_eight_nodelet_manager" args="manager" output="screen"/>
  <node pkg="nodelet" type="nodelet" name="make_world_models" args="load six_point_two_eight/make_world_models six_point_two_eight_nodelet_manager" output="screen">
    <remap from="velocity" to="$(arg velocity)"/>
    <remap from="points" to="$(arg points)_throttled"/>  <!-- 修正 -->
  </node>
  <!-- ここから追加 -->
  <node pkg="nodelet" type="nodelet" name="point_cloud_2_throttle" args="load six_point_two_eight/point_cloud_2_throttle six_point_two_eight_nodelet_manager" output="screen">
    <remap from="topic_in" to="$(arg points)"/>
    <remap from="topic_out" to="$(arg points)_throttled"/>
    <param name="update_rate" value="0.5"/>  <!-- 1秒に0.5回なので、2秒に1回。 -->
  </node>
</launch>

はい、これで深度センサーからのデータ量を減らすことができました。

PCLを使用して、点群を保存する

とまぁ、いろいろやってPointCloud2を取得できたわけですけれど、取得できただけではなんの意味もありません。でも、rosmsg sensor_msgs/PointCloud2で仕様を見ても、どう使えばよいのかよくわからないし……。

というわけで、PCLと省略して呼ばれるPoint Cloud Libraryを使ってみましょう。

PCLでできること

PCLのドキュメントを開くと、PCLの説明でよく使われる下の絵が表示されます。

PCLの重要なモジュール

filtersモジュールでフィルタ処理して特徴を保ったままで点の数を減らし、featuresモジュールで特徴量を推定して座標位置以外のモノサシを用意して、keypointsモジュールで特徴点を抽出して点群処理の際に注目すべき点を箇所を特定する。registrationモジュールで複数の点群を統合して大きな点群を作成し、kdtreeモジュールやoctreeモジュールで点を構造化する。segmentationモジュールとsample_concensusモジュールで点群を形状に基づいて分割し、surfaceモジュールで面を構築して3次元コンピューター・グラフィックスのライブラリに流す。recognitionモジュールで物体認識して処理対象を見つけてioモジュールで保存したり、visualizationモジュールでスクリーンに表示する……と（点群処理の素人が書いています。間違いがあったらごめんなさい）、とにかく多機能です。

PCLを使う場合の、コードの書き方

プログラマーとしてPCLについて知って置かなければならない最も重要なことは、「C++のライブラリであること」と「バージョン1.7.2ではC++14（C++11も）に対応していないこと」です。

ROSからPCLを扱うための便利機能が詰まったpcl_rosパッケージを使ったとしても、ROSの通信機能経由でPCLの機能を使えるわけではありません。ROSそのものはC++とPythonとCommon Lispとその他多数の言語で使えるのですけれど、PCLを使うプロジェクトではC++一択になります（PCLのPythonのバインディングもありますけれど、カバー範囲が狭い）。

C++14に対応していないというのはC++14の機能を使用して書かれていないという意味ではなく、PCLを使うコードをC++14モードでコンパイルすると、コンパイルはできるけど実行時に異常終了しちゃうということです（異常終了するかどうかは、使う機能次第ですけど。普通に使える機能もありました。PCL1.8でC++11でも大丈夫になるみたいなことは書いてあったのですけど、未確認です）。

なので、PCLを使用する部分は通常のコードとは分離して、しかもC++03で書かなければなりません。以下に、その具体的な方法を示します。とりあえずの処理として、PCLを使用して点群を保存してみました。

package.xmlとCMakeLists.txt

package.xmlとCMakeLists.txtに、pcl_rosパッケージを追加してください。

include/six_point_two_eight/point_cloud_utilities.h

コードの分離を確実にするために、C++14とPCLを完全に排除した*.hを作ります。関数宣言だけを、記述しました。

#ifndef SIX_POINT_TWO_EIGHT_POINT_CLOUD_UTILITIES_H
#define SIX_POINT_TWO_EIGHT_POINT_CLOUD_UTILITIES_H

#include <sensor_msgs/PointCloud2.h>

namespace six_point_two_eight {
  sensor_msgs::PointCloud2Ptr savePointCloud2File(sensor_msgs::PointCloud2ConstPtr points);
}

#endif

src/point_cloud_utiliites.cpp

PCLのioモジュールを使えば点群の保存なんて簡単……ではありますけど、悲しいことにそれなりの量のコードを書かなければなりません。PCLって機能はスゴイんですけど、プログラミング・スタイルが少し独特なんですよ。入力も出力も引数でやる方式なので、関数呼び出しのためだけに変数を定義したりreturnを別に書いたりしなければなりません。引数をスマート・ポインターで渡す時と参照で渡すときがあって、混乱しちゃいますし。

#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

#include "six_point_two_eight/point_cloud_utilities.h"

// 点群の型の整理。
typedef pcl::PointXYZRGB Point;  // 本稿では、色付きの点を使用します。あと、C++03なので、usingではなくtypedefを使用します。
typedef pcl::PointCloud<Point> PointCloud;

// 引数で指定した点群のファイル・パスを取得します。パスは、/tmp/six_point_two_eight_<点群の時刻>.pcdになります。
std::string filePathString(PointCloud::ConstPtr point_cloud) {
  std::stringstream file_path_stringstream;
  file_path_stringstream << "/tmp/six_point_two_eight_" << std::setfill('0') << std::setw(19) << pcl_conversions::fromPCL(point_cloud->header).stamp.toNSec() << ".pcd";

  return file_path_stringstream.str();
}

// 点群を保存します。
PointCloud::Ptr savePointCloudFile(PointCloud::ConstPtr point_cloud) {
  std::string file_path_string = filePathString(point_cloud);  // C++03なので、autoは使えません。

  pcl::io::savePCDFileBinary(file_path_string, *point_cloud);
  ROS_INFO_STREAM("PointCloud is saved to " << file_path_string);

  //  続けて別の関数を呼び出せるように、点群を返します。
  return PointCloud::Ptr(new PointCloud(*point_cloud));  // boost::shared_ptr<const X>からboost::shared_ptr<X>には変換できないので、新たにインスタンスを生成します。
}

// ROSの点群を、PCLの点群に変換します。
PointCloud::Ptr fromROSMsg(sensor_msgs::PointCloud2ConstPtr points) {
  PointCloud::Ptr converted_point_cloud(new PointCloud());
  pcl::fromROSMsg(*points, *converted_point_cloud);
    
  return converted_point_cloud;
}

// PCLの点群を、ROSの点群に変換します。
sensor_msgs::PointCloud2Ptr toROSMsg(PointCloud::ConstPtr point_cloud) {
  sensor_msgs::PointCloud2Ptr converted_points(new sensor_msgs::PointCloud2());
  pcl::toROSMsg(*point_cloud, *converted_points);
  
  return converted_points;
}

// 機能へのインターフェースを提供します。
sensor_msgs::PointCloud2Ptr six_point_two_eight::savePointCloud2File(sensor_msgs::PointCloud2ConstPtr points) {
  return toROSMsg(savePointCloudFile(fromROSMsg(points)));
}

include/six_point_two_eight/make_world_models.h

ロボット制御側の（C++14の）コードは以下の通り。point_cloud_utilitiesがPCLを隠蔽してくれていますから、こちらは簡単です。

#pragma once

#include <nodelet/nodelet.h>
#include <ros/ros.h>

#include <geometry_msgs/Twist.h>
#include <sensor_msgs/PointCloud2.h>

#include "six_point_two_eight/point_cloud_utilities.h"  // 追加。

namespace six_point_two_eight {
  inline auto createTwistMsg(float linear_x, float angular_z) {
    geometry_msgs::Twist msg;
    msg.linear.x = linear_x;
    msg.angular.z = angular_z;
    
    return msg;
  }
  
  class MakeWorldModels : public nodelet::Nodelet {
  private:
    ros::Publisher velocity_publisher_;
    ros::Subscriber points_subscriber_;
    ros::Timer timer_1_;
    ros::Timer timer_2_;

  public:
    void onInit() {
      velocity_publisher_ = getNodeHandle().advertise<geometry_msgs::Twist>("velocity", 1);

      points_subscriber_ = getNodeHandle().subscribe<sensor_msgs::PointCloud2>(
        "points", 10,
        [&](sensor_msgs::PointCloud2ConstPtr message) {
          savePointCloud2File(message);  // 点群を保存します。
        });

      timer_1_ = getNodeHandle().createTimer(
        ros::Duration(0.1),
        [&](const auto& message) {
          velocity_publisher_.publish(createTwistMsg(0.0, M_PI * 2 / 30));
        });
      
      timer_2_ = getNodeHandle().createTimer(
        ros::Duration(45.0),
        [&](const auto& message) {
          points_subscriber_.shutdown();
          timer_1_.stop();
          timer_2_.stop();
        });
    }
  };
}

CMakeLists.txt

ビルドを制御するために、CMakeLists.txtも変更しなければなりません。今回のように特定のファイルにだけコンパイル・オプションを設定したい場合は、set_cource_files_properties()を使用できます。以下のように、PCLに対応するために「-std=c++03 -Wno-deprecated」を設定してください。-std=c++03に加えて-Wno-deprecatedを指定しているのは、PCLのvisualizationモジュールが使用しているパッケージが使用を推奨されていない機能を使っているためです。ビルドのたびに警告が表示されて、しかも我々のコード以外が原因で対応不可能では、ストレスがたまるだけですもんね。

cmake_minimum_required(VERSION 2.8.3)
project(six_point_two_eight)

find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  nodelet
  nodelet_topic_tools
  pcl_ros
  roscpp
  sensor_msgs
)

# 略

catkin_package(
#  CATKIN_DEPENDS
)

add_compile_options(
  "-std=c++14"
)

# PCL1.7.2はC++14に対応していません。あと、visualizationがdeprecatedな機能を使用しています。コンパイル・オプションを指定して対応します。
set_source_files_properties(src/point_cloud_utilities.cpp  # 追加
  PROPERTIES COMPILE_FLAGS "-std=c++03 -Wno-deprecated"    # 追加
)                                                          # 追加

# 略

実行……してみたら、まだまだ機能が足りない

保存した点群は、pcl_viewerというコマンドで見ることができます。プログラムを実行し、Ctrl-cで止め、pcl_viewerで見てみましょう。

$ roslaunch six_point_two_eight turtlebot_driver.launch
$ catkin_make && rm -f /tmp/six_point_two_eight_*.pcd && roslaunch six_point_two_eight make_world_models.launch
# 処理が完了したら、Ctrl-cでプログラムを止める。

$ pcl_viewer /tmp/*.pcd
# 対象が画面に映るよう、rキーを押してカメラ・アングルをリセットする。
# 使いたは、hキーを押すとターミナルに表示される。
# マウスでグリグリしてみる。
# 飽きたら、qキーを押して終了させる。

ターミナル#2の1行目。コマンドを&&でつなぐと、「前のコマンドが成功したら後ろのコマンドを実行する」という指示になります。ビルドして、前の実行で生成された点群のファイルを消して、make_world_modelsを実行する（ただし、途中で失敗したら処理を止める）の意味になるわけ。この形で起動することで、ビルド忘れや過去の実行結果の混入を避けています。

で、その肝心のpcl_viewerで見た実行結果は、なんだか意味不明です。

make_world_models（その1）

どうしてこうなってしまったかというと、深度センサーからの相対座標の点群を、同じ座標系に重ねたから。以下のように1枚だけを表示するなら、普通に点群を見られます。

$ pcl_viewer `ls -s /tmp/six_point_two_eight_*.pcd | head -n 1`

というわけで、TurtleBotの姿勢に合わせて、適切に点群を移動/回転させてあげなければならないことが分かりました。

オドメトリー

少し話は変わりますけど、車やバイクにはオドメーターというのがあって、累計走行距離が分かるようになっています。あと、スピード・メーターで速度が表示できていて、速度は移動距離÷時間で、で、この距離ってどうやって測っているの？

車やバイクによっていくつかバリエーションはあるのですけれど、トランスミッション等の回転速度からタイヤの回転数を推測し、カタログ上のタイヤの外周のサイズをかけて移動距離を計算しています。同じことはロボットでも可能で、モーター等のセンサーから、タイヤが何回転したかが分かります。タイヤのサイズも設計書から分かる。だから、どれだけ移動したかが計算できるはず。TurtleBotのように左右にタイヤが付いている移動方式なら、左右のタイヤの回転数の差から、現在どちらを向いているのかも計算できるはず。

このように、モーター等のセンサーを使用して推定した位置情報を、オドメトリーと呼びます（車のオドメーターとは違って、位置と向きが分かる）。TurtleBotの場合は、/odomトピックでオドメトリーを取得できます。

さっそく、試してみましょう。

$ roslaunch six_point_two_eight turtlebot_driver.launch
$ roslaunch turtlebot_teleop keyboard_teleop.launch
$ rostopic echo /odom
header: 
  seq: 4443
  stamp: 
    secs: 1457333175
    nsecs: 603418136
  frame_id: odom
child_frame_id: base_footprint
pose: 
  pose: 
    position: 
      x: 0.20837154614
      y: -0.108868545114
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -0.305446027598
      w: 0.952209390956
  covariance: [0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0,
 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05]
twist: 
  twist: 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
 0.0, 0.0, 0.0]
---

ターミナル#2で適当にキーを押してTurtleBotを移動させると、ターミナル#3のpose.poseのpositionとorientationの値が変わっていきます。ROSの座標系はX軸が前でY軸が左でZ軸が上、回転は上から（Z軸のプラス側からマイナス側を）見て反時計回りで、/odomは起動時の状態からの相対座標/回転となっていることが分かります。

PCLを使用して、座標変換してダウンサンプリングしてフィルタリングする

でも、よっしゃー/odomトピックからオドメトリーを取得して点群を移動/回転させてやるぜーってのは、ごめんなさい、あまりうまくいきません。以下、その理由と対策をやって、ついでなので点群のダウンサンプリングとフィルタリングをやります。

TF

ROSのメッセージは大抵headerを含んでいて、headerのstampはそのメッセージが採取された時刻です。この値が異なっている場合の処理、たとえば5秒前に採取した点群を3秒前のオドメトリーで変換するような処理は、無意味です。だって、その間に移動したかもしれないんですから。

でも、もし7秒前のオドメトリーも知っているならどうでしょうか？　等速度運動をしていると仮定すれば、5秒前の位置は、7秒前の位置と3秒前の位置の中間だと推定することができます。ただし、そのためには過去の状態を複数保持し、間を補完する計算処理が必要です。メモリを食いつぶすわけにはいきませんから、ある程度古くなったメッセージを捨てる処理も必要です。でも、これを/odomトピックのサブスクライバーで作るのは大変そうです。深度センサーがロボット・アームの先に付いているような場合は、さらに面倒です。肩のサーボ・モーターがこの角度で肘のサーボ・モーターがあの角度だから、深度センサーはこの位置でこんな向きのはずだという計算も、とてもとても面倒くさそうです。

だから、ROSのパッケージのTFを使いましょう。TFは、上に挙げた面倒な計算処理をすべて実施してくれます。

TFの解説と使い方は、TF,TF2　完全理解にわかりやすくまとまっていて、すべてのROSプログラマーはこのスライドは絶対に見ておくべきなんですけれど、本稿の範囲だとpcl_rosパッケージのPCLとTFを取り持ってくれる機能を使えば大丈夫かも。この機能を使って、さくっとプログラミングしちゃいましょう。

package.xmlとCMakeLists.txt

package.xmlとCMakeLists.txtに、tfパッケージを追加してください。

include/six_point_two_eight/point_cloud_utilities.h

ごめんなさい。関数宣言だけでも、やっぱりTFの知識が必要だった……。まずは、TFのフレームの話をさせてください。

というのも、座標変換前の点群を調べてみると、深度センサーの位置を原点にして、前がZ軸で右がX軸、下がY軸になっているんですよ。

これはROSの座標系とは反転が必要なレベル（いわゆる右手系と左手系のレベル）で異なっているわけで、オドメトリーがどうのこうのの前に、まずはROSの座標系に変換しなければなりません。幸いなことにこの変換は、四元数と呼ばれる複素数を拡張した数体系を使えば、簡単に変換できるみたいです（x = -0.5、y = 0.5、z = -0.5、w = 0.5の四元数で変換できた）。で、次に問題になるのはTurtleBotの原点と深度センサーの原点が異なっていること。これは、ベクトルの引き算で変換できます。でも、こんな変換をいちいちプログラムしていくのは大変すぎます。なので、ロボットの構成要素と構成要素を、必要な座標系の変換でつなぐことにしましょう。camera_rgb_optical_frame→（変換）→camera_rgb_frame→（変換）→base_linkみたいな感じ。この方式ならプログラミングではなくてデータ作成で済むので、とても楽になります。

ただ、base_linkには深度センサー以外の構成要素もつながっていて、例えばwheel_right_link（右の車輪）なんてのもあります。そう考えると、構成要素の関係はリストではなくて木構造が適切かなぁと。で、さらに考えてみると、オドメトリーとロボット本体の関係も、この木構造で表現できます。変換が毎回同じか、移動で変わるかの違いだけですもんね。というわけで、TFではこの構成要素をフレームと呼んで木構造にして、座標変換でつないでいきます。TureltBotのドライバーを動かした状態だと、odomフレームをルートととする、camera_rgb_optical_frameフレームやwheel_right_linkフレームを葉に持つ木構造が作られています。

TFで座標変換する際には、このフレームを使用します。どのフレームの座標系からどのフレームの座標系に変換するかを指定するわけ。指定するフレームは木のどこにあっても大丈夫です。木構造を辿ればよいわけですから。今回の場合、点群はcamera_rgb_optical_frameの座標系であることが分かっている（というか、ROSではheader.frame_idにメッセージのフレームが設定されている）ので、変換先のフレームだけ指定すればよいことになります。なので、二番目の引数としてtarget_frameを渡しています。

// 略

namespace six_point_two_eight {
  sensor_msgs::PointCloud2Ptr savePointCloud2File(sensor_msgs::PointCloud2ConstPtr points);
  sensor_msgs::PointCloud2Ptr transformPointCloud2(sensor_msgs::PointCloud2ConstPtr points, const std::string& target_frame, double x_adjust = 0.0);  // 追加。
}

// 略

残りの引数のx_adjustは、プログラムを作成して試してみたら奥行きに少しずれがったから追加してみました。こんな引数は不要なはずなのですけれど、ご容赦ください。

src/point_cloud_utilities.cpp

なんだかとても長いコードになってしまいました……。理由は、3つの処理が必要だったから（本当は処理毎に関数に抽出したかったのですけれど、そうするとここだけプログラミング・スタイルが変わって保守性が落ちてしまう）。

というわけで、処理その1。奥行きの補正です。補正しないバージョンのプログラムでデータを調べてみたら、なんだか少しだけずれていたので追加しました。

#include <boost/foreach.hpp>  // 追加。
#include <pcl/filters/filter.h>  // 追加。
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>  // 追加。
#include <tf/transform_listener.h>  // 追加。

#include "six_point_two_eight/point_cloud_utilities.h"

typedef pcl::PointXYZRGB Point;
typedef pcl::PointCloud<Point> PointCloud;

tf::TransformListener transform_listener_;  // 追加。

// 略

// 点群をtarget_frameの座標系に変換します。その際に、x_adjustで指定された値で、奥行きも補正します。
PointCloud::Ptr transformPointCloud(PointCloud::ConstPtr point_cloud, const std::string& target_frame, double x_adjust) {
  // 奥行きがずれているようだったので、補正します。
  PointCloud::Ptr x_adjusted_point_cloud(new PointCloud(*point_cloud));
  BOOST_FOREACH(Point& point, *x_adjusted_point_cloud) {
    point.z += x_adjust;  // 深度センサーの生データは、実は座標系が異なります。右がX軸で下がY軸、前がZ軸なんです。
  }

  // TFは姿勢情報と姿勢情報の間しか補完できない（最新の姿勢情報よりも後の時刻で問い合わせるとエラーになる）ので、点群よりも後の姿勢情報が来るまで待ちます。
  std_msgs::Header header = pcl_conversions::fromPCL(point_cloud->header);
  if (!transform_listener_.waitForTransform(target_frame, header.frame_id, header.stamp, ros::Duration(1.0))) {
    ROS_WARN_STREAM("Can't wait...");
    throw std::exception();
  }
  
  // 内部でTFを使用して、座標変換します。
  PointCloud::Ptr transformed_point_cloud(new PointCloud());
  if (!pcl_ros::transformPointCloud(target_frame, *x_adjusted_point_cloud, *transformed_point_cloud, transform_listener_)) {
    ROS_WARN("Transform failed...");
    throw std::exception();
  }

  // 座標変換した点群を返します。
  return transformed_point_cloud;
}

// 略

sensor_msgs::PointCloud2Ptr six_point_two_eight::transformPointCloud2(sensor_msgs::PointCloud2ConstPtr points, const std::string& target_frame, double x_adjust) {
  return toROSMsg(transformPointCloud(fromROSMsg(points), target_frame, x_adjust));
}

src/make_world_models.cpp

// 略

namespace six_point_two_eight {
  // 略
  
  class MakeWorldModels : public nodelet::Nodelet {
    // 略
    
    void onInit() {
      velocity_publisher_ = getNodeHandle().advertise<geometry_msgs::Twist>("velocity", 1);

      points_subscriber_ = getNodeHandle().subscribe<sensor_msgs::PointCloud2>(
        "points", 10,
        [&](sensor_msgs::PointCloud2ConstPtr message) {
          try {                                 // 追加。
            savePointCloud2File(
              transformPointCloud2(             // 追加。
                message,
                "odom",                         // 追加。
                -0.05));                        // 追加。
            
          } catch (const std::exception& ex) {  // 追加。
            ROS_WARN_STREAM(ex.what());         // 追加。
          }                                     // 追加。
        });

      timer_1_ = getNodeHandle().createTimer(
        ros::Duration(0.1),
        [&](const auto& message) {
          velocity_publisher_.publish(createTwistMsg(0.0, M_PI * 2 / 30));
        });
      
      timer_2_ = getNodeHandle().createTimer(
        ros::Duration(45.0),
        [&](const auto& message) {
          points_subscriber_.shutdown();
          timer_1_.stop();
          timer_2_.stop();
        });
    }
    
    // 略
}

対象物の周囲360°からの3D点群を作成する

オドメトリー再び

actionlib

PCLを使用して、床を除去する

動作確認

3D点群を統合する

PCLを使用して、点群をレジスターする

パラメーター

Boost.Range

動作確認

おわりに

  1. 2015年にROS勉強記録で有名な小倉崇さんによる「ROSではじめるロボットプログラミング」が出版され、ROSそのものやPythonでのプログラミングの日本語文書は充実しました。他にも、「詳説ROSロボットプログラミング -導入からSLAM、Gazebo、MoveItまで-」のように、自律移動やロボット・アームの操作までの広い全体像を解説する無料の書籍もあります。でも、C++14を用いたプログラミングについては、2016年3月現在ではまだ不十分に思えます。↩

  2. 本稿では、プログラミング部分のみを述べます。セットアップやROSのパッケージ群については、ROS Wikiや「ROSではじめるロボットプログラミング」を参照してください。↩

  3. モーターは電磁石と永久磁石が引き合ったり反発したりして回っていて、電磁石の極を入れ替える（+/-を入れ替える）ことで回転を継続させています。回転で機械的に+/-を切り替えるのが普通のモーターで、電子回路で+/-を切り替えるのがブラシレス・モーターになります。↩

  4. nameの値は、ユニークな値なら何でも構いません。typeと同じ値を使う場合が多いようです。↩

  5. turtiesimにはdraw_square等のノードもあり、キーボードで操作したり自律移動させたりして楽しめます。なので、ごめんなさい。無条件にturtle_teleop_keyを起動してしまう今回のlaunchファイルは、楽ちんではありますけど正しくはありません。turtlesim_nodeの起動とturtle_teleop_keyの起動は分けておくのが、turtlesimの使い方的に正しい形でしょう。↩

  6. 実は、package.xmlとCMakeLists.txtを修正しなくてもビルドできちゃう場合も多いです。でも、package.xmlに依存するパッケージが正しく入っていないと他の環境でどのパッケージが必要なのか分からなくてビルドできなくなっちゃいますし、CMakeLists.txtのfind_package()はパッケージ情報を環境変数に設定してくれてそれを使ってビルドを制御するかもしれませんから、面倒でも登録するようにしてください。本稿でも、真面目に追加していきます。↩

  7. getPrivateNodeHandle().advertise<geometry_msgs::Twist>("/mobile_base/commands/velocity", 1)（トピック名を「/」で始める）にすれば問題ないように思えるかもしれませんが、ロボットが2台あって/turtlebot_1/mobile_base...と/turtlebot_2/mobile_base...に分けているような場合に問題が発生します。トピック名はノードが所属している名前空間からの相対パスで解釈されますので、/turtlebot_1/make_world_modelsと/turtlebot_2/make_world_modelsとしてノードを起動し、「/」から始まらないトピック名にしておけば、同じプログラムで2台のTurtleBotを扱えて便利なんですよ。↩